/*
 * processCloud.cpp
 *
 *  Created on: 20 mai 2012
 *      Author: flolom
 */

#include "processCloud.h"

#include <iostream>
#include <map>
#include <set>
#include <MapGrid.h>
#include <Robot.h>
#include <Geometry/Cercle.h>
#include <Geometry/Triangle.h>
#include <Geometry/Point2D.h>
#include <list>


#include <QFile>

#include <ctime>

using namespace livia;
using namespace livgeo;
using namespace std;

void acquerirNuagePt(MapGrid* grid, Robot* rob)
{
	// Requete connexion
	QFile file("tampon");
	file.open(IO_Raw | IO_ReadWrite);
	file.close();

	QFile file("test.txt");
	while (!file.is_open()) ;



	set<Point2D, point2Dcomp> pts;
	// le traiter

	// XYGridList modifsMap = algoTropGG( ... )
	// grid->set(modifsMap);
	// Network.send( ... )

}

list<Point> getPointCloud(QFile& file)
{
	QFile  file("test.txt");
	file.open(QIODevice::ReadOnly);
	QDataStream in(&file);

	list<Point> l;

	int size;

	in >> size;

	for (int i=0; i< size; i++)
	{
		Point tmp;
		in >> tmp.x >> tmp.y >> tmp.z >> tmp.r >> tmp.g >> tmp.b;
	    l.push_back(tmp);
	}

	file.close();
	tampon.remove();

	return l;
}

void processPts(/*pcl point set, */set<Point2D, point2Dcomp>& out, MapGrid* grid)
{

	/*for (;;)
	{
		if ( z >1 && z < Robot::HEIGHT && x > 0 && x < grid->getWidth()*grid->getScale() &&
				y>0 && y < grid->getHeight()*grid->getScale())
		{
			Point2D pt(x/grid->getScale(), y/grid->getScale());
			out.insert(pt);
		}
	}*/
}

GridModifs algoTropGG(set<Point2D, point2Dcomp>& obstacles,
		MapGrid& grid, Robot& robot) {

	// todo prendre en paramètres
	//list<Point2D> obstacles;
	//obstacles.push_back(Point2D(50, 80));
	//obstacles.push_back(Point2D(51, 80));

	// todo donner en retour
	XYGridList listModifs;

	clock_t timeBefore = clock();

	// Carte de 10x10 mètres, avec une résolution au millimètre près
	//MapGrid grid(100, 100, 2.0);

	cout << "Temps de création de la carte: "<< (double)(clock() - timeBefore) / CLOCKS_PER_SEC << endl;

	// Robot positionné au centre de la carte, orienté vers l'axe Y
	//Robot robot(50, 50, 0.0f);

	// Angle de vu du robot
	Triangle angleVu(robot.getPosition(), robot.getAngle(), Robot::FOV,
			Robot::MAX_VIEW);

	// ---------------------------------------------------------------- Etape 1
	// Sélection des cases concernées

	// Création des structures temporaires contenant les éléments a mettre à
	// VIEW au final et des éléments à mettre à crossable.
	// Le vecteur correspond à une ligne sur l'axe Y, tandis que la map
	// correspond à tous les éléments sur l'axe X.
	vector< AxeXGridElem > tempGrid;
	vector< AxeXGridElem > tempCrossable;


	// coordonnées de la bounding box
	Point2D botL, topR;
	angleVu.createBoundingBox(botL, topR);

	// offset entre le 0 absolu et le 0 de la bounding box
	int offsetX = botL.getX() / grid.getScale();
	int offsetY = botL.getY() / grid.getScale();

//	cout << "offsetX: "<<offsetX << endl;
//	cout << "offsetY: "<<offsetY << endl;


	// DEBUG ONLY
	timeBefore = clock();

	// Parcours de toutes les cases de la bounding box
	for (int j = offsetY;
			j < (int)(topR.getY() / grid.getScale());
			j++)
	{
		if (j<0 || j>=grid.getHeight()) continue;
		// Ajout d'une nouvelle ligne
		tempGrid.push_back( AxeXGridElem() );
		tempCrossable.push_back( AxeXGridElem() );

		// iterateur sur les derniers éléments insérés : optimisation uniquement !
		AxeXGridElem::iterator prevGrid;
		AxeXGridElem::iterator prevCrossable;
		bool first = true;

		for (int i = offsetX;
				i < (int)(topR.getX() / grid.getScale());
				i++)
		{
			if (i<0 || i>=grid.getWidth()) continue;
			Point2D current( (i+0.5)*grid.getScale(), (j+0.5)*grid.getScale());
			if (angleVu.contains(current) )
			{
				if (first)
				{
					// Première insertion : normale

					prevGrid = (tempGrid[ j-offsetY ].insert(make_pair(i, &grid.get(i, j)))).first;
					prevCrossable = (tempCrossable[ j-offsetY ].insert(make_pair(i, &grid.get(i, j)))).first;
					first = false;
				}
				else
				{
					// N-ième insertion : on insère à partir d'un itérateur sur la dernière insertion : gain de temps

					prevGrid = tempGrid[ j-offsetY ].insert(prevGrid, make_pair(i, &grid.get(i, j)));
					prevCrossable = tempCrossable[ j-offsetY ].insert(prevCrossable, make_pair(i, &grid.get(i, j)));
				}
			}
			else if (!first)
			{
				break;
			}
		}
		cout << "Ligne " << j << " : " << tempGrid[j-offsetY].size() << "elements" << endl;
	}

	cout << "Temps de l'étape 1: "<< (double)(clock() - timeBefore) / CLOCKS_PER_SEC << endl;

	// --------------------------------------------------------------- Etape 2

	GridModifs modifs;

	// radius d'une case depuis son center
	float r = grid.getScale();///sqrt(2);

	set<Point2D>::iterator iter;
	for (iter = obstacles.begin(); iter != obstacles.end(); ++iter)
	{
		cout << "----------------------------------------"<<endl;

		GridView viewTmp;
		viewTmp.x = iter->getX();
		viewTmp.y = iter->getY();
		viewTmp.grid = OBSTACLE;

		modifs.modifsView.push_back(viewTmp);

		// ZONE CROSSABLE / NOT CROSSABLE


		Cercle zoneNotCrossable(*iter, Robot::MAX_RADIUS);
		Point2D botL, topR;
		zoneNotCrossable.createBoundingBox(botL, topR);

		cout << botL.getY() / grid.getScale() << " - " << topR.getY() / grid.getScale()<<endl;
		for (int j = botL.getY() / grid.getScale(); j < topR.getY() / grid.getScale(); j++)
		{
			if (j<0 || j>= grid.getHeight()) continue;

			AxeXGridElem& mapX = tempCrossable[j - offsetY];
			//cout << "Taille X:" << mapX.size()<<endl;

			AxeXGridElem::iterator iterMinX = mapX.lower_bound(botL.getX() / grid.getScale() );

			// No need to compute here
			if (iterMinX == mapX.end())
				continue;

			AxeXGridElem::iterator iterX;
			AxeXGridElem::iterator minDel=mapX.end();
			bool firstContained = false;

			//cout << j;
			for (iterX = iterMinX; iterX->first <= topR.getX() / grid.getScale();++iterX)
			{
				//std::cout << "o";
				if (zoneNotCrossable.contains(Point2D((iterX->first+0.5)*grid.getScale(), (j+0.5)*grid.getScale())) )
				{
					if (!firstContained)
						minDel=iterX;

					firstContained = true;

					std::cout << "("<<iterX->first<<","<<j<<") : not crossable" << endl;

					GridCrossable crossTmp;
					crossTmp.x = iterX->first;
					crossTmp.y = j;
					crossTmp.crossable = false;

					modifs.modifsCross.push_back(crossTmp);
					// todo ajouter à la liste des modifications

				}
				else if (firstContained)
					break;
			}
			if (minDel != mapX.end())
				mapX.erase(minDel, iterX);
		}

		// ZONE VIEW / NOT VIEWED
		Vector2D robot_obst ((*iter) - robot.getPosition());
		float D = robot_obst.norm2();
		float theta = atan(r/D);
		Vector2D d1 = robot_obst.rotate(robot.getPosition(), theta);
		Vector2D d2 = robot_obst.rotate(robot.getPosition(), -theta);

		Point2D max1, max2;
		angleVu.intersectWith(d1, *iter, (*iter)+d1, max1);
		angleVu.intersectWith(d2, *iter, (*iter)+d2, max2);

		Triangle tmp(*iter, max1, max2);

		tmp.createBoundingBox(botL, topR);

		for (unsigned int j = botL.getY() / grid.getScale(); j < topR.getY() / grid.getScale(); j++)
		{
			if (j<0 || j>= grid.getHeight()) continue;

			AxeXGridElem& mapX = tempGrid[j - offsetY];
			AxeXGridElem::iterator iterMinX = mapX.lower_bound(botL.getX() / grid.getScale() );

			// No need to compute here
			if (iterMinX == mapX.end())
				continue;

			AxeXGridElem::iterator iterX;
			AxeXGridElem::iterator minDel=mapX.end();
			bool firstContained = false;

//			cout << j << " - Taille X:" << mapX.size()<<endl;

			for (iterX = iterMinX; iterX->first <= topR.getX() / grid.getScale();++iterX)
			{
				Point2D pt((iterX->first+0.5)*grid.getScale(), (j+0.5)*grid.getScale());
				Vector2D vect = pt-(*iter);

				float angle1 = Vector2D::angleBetween(d1, vect);
				float angle2 = Vector2D::angleBetween(d2, vect);

				//cout << "(" << iterX->first << ","<< j << ") " << "angle1=" << angle1 << " angle2=" << angle2 << endl;
				// Point shouldn't be viewed
				if (angle1 <= 0 && angle2 >= 0)
				{
					std::cout << "("<<iterX->first<<","<<j<<") : pas visible" << endl;
					if (!firstContained)
						minDel = iterX;
					firstContained=true;
				}
				else if (firstContained || iterX == mapX.end())
					break;
			}

			if (minDel != mapX.end()) {
				mapX.erase(minDel, iterX);
			}
		}
		// Mettre l'objet en
	}

	// ---------------------------------- Mise a jour des cases
	for (int j=0; j<tempGrid.size(); j++)
	{
		AxeXGridElem& mapX = tempGrid[j];
		AxeXGridElem::iterator iterX;

		for (iterX = mapX.begin(); iterX!=mapX.end();++iterX)
		{
			if (iterX->second->grid == NOT_VIEWED)
			{
				GridView viewTmp;
				viewTmp.x = iterX->first;
				viewTmp.y = j;
				viewTmp.grid = VIEWED;
				modifs.modifsView.push_back(viewTmp);
			}
		}
	}

	cout << "fin !"<<endl;
	return modifs;
}
